/********************************** (C) COPYRIGHT *******************************
* File Name          : pll.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/12/9
* Description        : pll相关接口在这里实现
*******************************************************************************/
/***********************************头文件区域***********************************/
#include "pll.h"
#include "interact.h"
#include "string.h"

/***********************************类型定义*********************************/


/***********************************宏定义区***********************************/


/******************************** *全局变量区**********************************/


static void doPll(struct pll *pPll, _iq coffSin, _iq coffCos)
{
    _iq error;
    _iq cosV;
    _iq sinV;
    _iq _omega;
    cosV = _IQcos(pPll->theta);
    sinV = _IQsin(pPll->theta);
    
    error = _IQmpy(coffSin, cosV) - _IQmpy(coffCos, sinV);
    error = (error > pPll->errThre) ? (pPll->errThre) : error;
    error = (error < -pPll->errThre) ? (-pPll->errThre) : error;

    _omega = (pPll->pid->calc(pPll->pid, error >> 8) << 8);
    
    pPll->omega = _IQmpy(_IQ(1) - pPll->kslf, pPll->omega) + _IQmpy(pPll->kslf, _omega);
    //pPll->omega = _omega;
    
    pPll->theta +=  _IQmpy(pPll->omega, pPll->Ts);

    if (pPll->theta > pPll->thetaLimit)
    {
        pPll->theta -= pPll->thetaLimit;
    }
    else if (pPll->theta < -pPll->thetaLimit)
    {
        pPll->theta += pPll->thetaLimit;
    }

    // printMat[0] = (float)(_omega >> 12);
    // printMat[1] = (float)(pPll->omega >> 12);
    // printMat[2] = (float)(pPll->theta >> 12);

    // printData(printMat,3);
}


/*********************************************************************
 * @fn      pllCreate
 *
 * @brief   pll构造函数，
 *              void
 *
 * @return  FUNC_SOK，成功  FUNC_FAIL，失败
 *          
 */
pll_ * pllCreate(void)
{
    pll_ *pPll = NULL;

    pPll = (pll_ *)malloc(sizeof(pll_));
    if (!pPll)
    {
        return NULL;
    }
    memset((void *)pPll, 0, sizeof(pll_));

    pPll->pid = pidCreate(_IQ12(8.35), _IQ12(0.65), _IQ12(0.001));
    // pPll->pid = pidCreate(_IQ12(48.35), _IQ12(2.201), _IQ12(0.010));
    pPll->kslf = _IQ(PLL_KSLF);
    pPll->Ts = _IQ((double)FOC_CALC_CYCLE / 1000000);
    pPll->errThre = _IQ(PLL_PI_DIV6);
    pPll->thetaLimit = _IQ(PLL_2MPY_PI);

    pPll->pll = doPll;
    
    return pPll;
}

